clear all;

% Вывод отладочной информации
global DEBUG_LEVEL;
DEBUG_LEVEL = 1;
global SUBPLOTS_X;
SUBPLOTS_X = 2;
global SUBPLOTS_Y;
SUBPLOTS_Y = 2;
global DO_PLOT;
DO_PLOT = 1;

% Период обзора [c]
global g_period;
g_period = 1;

% Размер кванта [рад]
global g_delta_phi;
g_delta_phi = pi/2^5;

% Размер кванта [м]
global g_delta_r;
g_delta_r = 150;

% Вероятность ложного измерения в кванте
global g_prob_false;
g_prob_false = 0.1;

% СКО ошибок измерения [м]
global g_sigma_r;
g_sigma_r = g_delta_r;

% СКО ошибок измерения, [рад]
global g_sigma_phi;
g_sigma_phi = g_delta_phi;

% Количество анализируемых обзоров
global g_survey_num;
g_survey_num = 10;

% Поле обзора [м]
global g_max_radius;
g_max_radius = 1.2e3;

% Максимальная скорость
global g_max_vel;
g_max_vel = 1e3;

% Количество квантов в одном обзоре
global g_quant_num;
g_quant_num = 2 * pi / g_delta_phi * ...
    (g_max_radius / g_delta_r);

% Множество квантов измерений
global g_quants;
% g_quants = dlmread('q.txt');
v_true = -300 * 1i;
r_true = (g_max_radius - 100) * 1i;
debug_print(1, 'Истинное значение: v = %f, phi = %f\n', ...
    abs(v_true), angle(v_true));
g_quants = quants(r_true, v_true);

% Расстановка квантов в ГСК
quant_centers_gsk = quant_center(0);

% Начало помещаем в точку с наиболее удаленной целью (для примера)
starting = max(quant_centers_gsk .* g_quants(1,:)');

% Координаты квантов в системе координат цели
quant_centers_targ = quant_center(starting);

debug_print(2, 'Центры квантов:\n');
debug_disp(2, quant_centers_targ);

if (DO_PLOT == 1)
    clf();
end;

v0 = 300;
phi0 = -pi/2;
v = v0;
phi = phi0;
debug_print(1, 'Начальные оценки: v0 = %f, phi0 = %f\n', v0, phi0);

for survey=1:g_survey_num
    w = weights(quant_centers_targ, v0, phi0, survey);
    if (DO_PLOT == 1)
        draw_scene(starting, quant_centers_targ, survey);
        title('1');
        draw_weights(quant_centers_targ, w, survey);
        title('2');
        %title(sprintf( ...
        %    'W (k = %d, v = %f, phi_v = %f)', ...
        %    survey, v0, phi0));
    end
    [v, phi] = estimate(quant_centers_targ, v0, phi0, survey);
    if (DO_PLOT == 1)
        draw_results(v, phi, survey);
    end
    print('-dpng', sprintf('%2d.png', survey));
    % pause(0.2);
end

debug_print(1, 'Конечные оценки: v = %f, phi = %f\n', v, phi);